/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file dataToXML.cxx
  \brief class implemenation XML generator

  \author Jack Elston
  $Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <iostream>
#include <stdio.h>
#include <math.h>

#include "fpAp.h"
#include "utilities.h"

/*<-------Defines--------->*/
/*<------End Defines------>*/

/*<---Global Variables---->*/
/*! \brief determines level of output to user. */
extern bool verbose;
uint8_t fpap_mode = AP_PRELAUNCH;
uint8_t fpob_mode = OB_WAYPT;
/*<-End Global Variables-->*/


/************************************************************\
|                  DataToXML_Parser::init                    |
\************************************************************/
/*!
  \brief the initializing function for the XML generating thread.
  \param shm shared memory struct between the gateway threads.
  \pre instance of DataToXML_Parser has been created
  \post the XML utils have been intialized and the inetTxFifo
  and SAVRxFifo have been opened
  \return TRUE on successful completion of initialization, FALSE otherwise
*/

FPAPThread::FPAPThread(String name, float v_min, float v_max, float dpsi_max) : ThreadedPipe(name)
{
	srand(time(0));
	waypoints[0].number   = 0;
	waypoints[0].next     = 0;
	waypoints[0].orbitRad = 100;
	waypoints[0].pos.lat  = 0;
	waypoints[0].pos.lon  = 0;
	waypoints[0].pos.alt  = 1800;

	current_waypoint                = 0;

	limits.v_max = v_max;
	limits.v_min = v_min;
	limits.dpsi_max = dpsi_max*M_PI/180;

	ap_rates.turn_rate = 0;
	ap_rates.velocity = 0;
}

bool FPAPThread::init(ShmStruct * shmStructPtr)
{
	ThreadedPipe::init(shmStructPtr);

	last_telem_tx.stamp();
	return true;
}

void FPAPThread::update()
{
	int maxFD, val, n=0;
	fd_set readFDs, writeFDs;
	struct timeval timeout;

	/*<------Select FDs------->*/
	FD_ZERO(&readFDs);
	FD_ZERO(&writeFDs);

	FD_SET(fifo_FDs[INBOARD_RX],&readFDs);
	FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);
	maxFD = fifo_FDs[INBOARD_RX];
	maxFD = (fifo_FDs[INBOARD_TX] > maxFD ? fifo_FDs[INBOARD_TX] : maxFD);

	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	val = select(maxFD + 1, &readFDs, &writeFDs, NULL, &timeout);

	Client * me = shmPtr->clients.getClient(0);

	if( val < 0 ) {
		if (errno != EINTR)
			cout << getName() << "::update() - select, " <<  strerror(errno) << endl;
	}
	else if(val == 0) {
	}
	/*<----One FD is Active--->*/
	else {
		/*<---RX Operations--->*/
		if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs)) {
			if((n = readFifo(INBOARD_RX)) > 0) {
				uint16_t length = CMD_SIZE(fifo_msg.data);
				if( length != fifo_msg.size) {
					cout << getName() << "::update() - error, length of command != length of buffer" << endl
						<< "\t CMD_SIZE=" << (int)length << " fifo_msg.size=" << (int)fifo_msg.size << endl;

					if( length < 1000 && length <= fifo_msg.size ){
						if( C_calcCheckSum((uint8_t *)buf) == 0 ) 
							cout << "\tCMD: is valid" << endl;
						else
							cout << "\tCMD: is NOT valid" << endl;
					}

					return;
				}

				Command cmd((uint8_t *)fifo_msg.data);

				if( !cmd.isValid() )
					cout << getName() << "::update() - error, invalid command" << endl << cmd ;
				else {
					Client * a_client;
					a_client = shmPtr->clients.getClient(fifo_msg.src);

					uint8_t * dataPtr = CMD_DATAPTR(fifo_msg.data);

					switch(cmd.cmd() ) {
						case CMD_FP: {
								fpap_mode = AP_FLYING;
								fpob_mode = OB_WAYPT;
								int num_waypts = cmd.size()/sizeof(Waypt_t);
								if(num_waypts > NUM_WAYPOINTS)
									num_waypts = NUM_WAYPOINTS;

								memcpy(&waypoints,dataPtr,num_waypts*sizeof(Waypt_t));
								current_waypoint = 0;

								ACK(INBOARD_TX);

							} break;
					}
				}
			}
		}
		/*<---TX Operations--->*/
		if(FD_ISSET(fifo_FDs[INBOARD_TX], &writeFDs)) {
			currentStamp.stamp();
			if((currentStamp - last_telem_tx ) > (RATES_CONTROL_PERIOD * SEC2MICRO) ) {
				if(fpap_mode == AP_FLYING) {
					moveUA();
					Command cmd;
					char * dataPtr = (char *)CMD_DATAPTR(fifo_msg.data);

					CMD_CMD((fifo_msg.data)) = CMD_AP_RATES;
					CMD_NUM((fifo_msg.data)) = sizeof(APRates_t);
					CopyStructToBuf(ap_rates, dataPtr);
					CMD_CSUM(fifo_msg.data) = 0;
					CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

					fifo_msg.size = CMD_SIZE(fifo_msg.data);

          fifo_msg.dest.s_addr = me->address[0].s_addr;
          fifo_msg.src.s_addr  = me->address[0].s_addr;
					writeFifo(INBOARD_TX);

					last_telem_tx.stamp();
				}

			}
		}
	}
}

void FPAPThread::moveUA()
{
	float psi_rate = 0;

	float psides = 0;
	float vdes = 0;
	Velocity_t vel;
	float psidotdes = 0;

	Client * me = shmPtr->clients.getClient(0);
	float r = GPSRange( me->telemetry.gps.pos, waypoints[current_waypoint].pos);

  vdes = limits.v_max;

  //if(vdes > limits.v_max) vdes = limits.v_max;
  //else if(vdes < limits.v_min) vdes = limits.v_min;

	if(fpap_mode == AP_FLYING && (fpob_mode == OB_WAYPT || fpob_mode == OB_OFF) ) {
		vectorField(me->telemetry.gps.pos, me->telemetry.gps.track, waypoints[current_waypoint].pos, 
				waypoints[current_waypoint].orbitRad, vdes, &vel, &psides, &psidotdes);
		psi_rate = getTurnRate((float)me->telemetry.gps.track*M_PI/180, psides, psidotdes, 1.0);

	}

  //if(psi_rate > limits.dpsi_max) psi_rate = limits.dpsi_max;
  //else if(psi_rate < -limits.dpsi_max) psi_rate = -limits.dpsi_max;

  ap_rates.velocity = vdes;
  ap_rates.turn_rate = psi_rate;
  
	if(waypoints[current_waypoint].orbitRad == 0)
		if(r < (2*vdes)) {
			current_waypoint = waypoints[current_waypoint].next;
		}
}

FPAPThread::~FPAPThread()
{
}

